#include "Tria3SmallDrillMem.h"

#include "SGFem/Integration/IntegrationFormula/Gauss2D1X1P.h"

using SG::DataStructure::Common::Real;
using SG::Algebra::Matrix;

std::size_t Tria3SmallDrillMem::s_nNode = 3;

void Tria3SmallDrillMem::Initialize ()
{}

Matrix Tria3SmallDrillMem::ComputeStiffness 
(
const Matrix& coord, 
const SG::Algebra::Matrix& Cm
) const
{
    Matrix ke (9, 9);

    Matrix B (3, 9);
    ComputeStrainMatrix (coord, _OUT B);

    const Real& x2 = coord(1, 0);
    const Real& x3 = coord(2, 0);
    const Real& y3 = coord(2, 1);
    Real A2 = y3 * x2;
    ke = B.Transpose() * Cm * B * (A2 * 0.5);

    Matrix Q (1, 9,{- (x2 - x3) / (A2 * 2.0),  y3 / (A2 * 2.0), 1.0 / 3.0, 
                      -x3 / (A2 * 2.0),       -y3 / (A2 * 2.0), 1.0 / 3.0, 
                       x2 / (A2 * 2.0),                    0.0, 1.0 / 3.0});

    auto GV = A2 / 2.0 * Cm (2, 2);

    // drill
    const Real KROT6 =100.0;
    Real delta2{1.0e-4 * KROT6};
    Real tmp = delta2 * GV;
    Matrix Sr =  Q.Transpose() * (Q * tmp);

    ke += Sr;
    auto lambda =  1.0e-10 * GV * KROT6;
    ke(2, 2) += lambda;
    ke(5, 5) += lambda;
    ke(8, 8) += lambda;
 
    return ke;
}

void Tria3SmallDrillMem::ComputeStrainMatrix(const Matrix& coord, _OUT Matrix& B) const
{
    Real   x2 = coord (1, 0);
    Real   x3 = coord (2, 0);
    Real   y3 = coord (2, 1);
    
    B = Matrix(3, 9);
    
    B (0, 0) = -1.0 / x2;
    B (0, 3) = 1.0 / x2;
    B (1, 1) = (x3 - x2) / (x2 * y3);
    B (1, 4) = -x3 / (x2 * y3);
    B (1, 7) = 1.0 / y3;
    B (2, 0) = (x3 - x2) / (x2 * y3);
    B (2, 1) = -1.0 / x2;
    B (2, 3) = -x3 / (x2 * y3);
    B (2, 4) = 1.0 / x2;
    B (2, 6) = 1.0 / y3;
}